#!/usr/bin/python

import numpy as np
from numpy import sin,cos,pi,dot,sqrt

import matplotlib.pyplot as pl


GAP = 0

def Zr_primary( x, y ):
  "Given a point in the x-y plane, returns the point on the primary mirror surface\
  defined in Final Mirror Surface Shapes for the Atacama Cosmology Telescope from\
  Joe Fowler & Mike Niemack. "
  R  = -10000.0       #radius of curvature at the vertex, in mm 
  Zv =      0.0       #z axis offset of the vertex, in mm
  K  =     -0.940935  #conic constant K = -e^2
  
  result = Zv + (x*x+y*y)/R * 1./( 1. + sqrt( 1. - (1. + K) * (x*x+y*y)/(R*R)  )  )

  return result


def get_XYZ_corners( panel_geometry_file='data/panel_geometry.dat' ):
  "Returns a numpy array like object containing an Nx4x3 matrix. Each row\
   in the matrix represents the 4 corners of a panel, while each is defined\
   in the x-y-z space using a 3x1 numpy array.\
   \
   input:  path to the file containing the panel geometry definition\
   \
   output: Nx4x3 numpy array."

    
  #Get data from text file
  panel_data = np.loadtxt( panel_geometry_file )

  all_corners = []

  #Iterate over rows of panels
  for panel_row in panel_data:
    #internal radius
    rin  = panel_row[1]
    #external radius
    rout = panel_row[2]
    #width
    w    = panel_row[5]
    #number of panels in the row
    ncol = (int)(panel_row[7])
    #subtended angle by the panel
    alpha = np.arctan(0.5*w/rout+GAP/rout)
 
    for i in range(-ncol/2+1,ncol/2+1):
      tht = pi/2. - 2*alpha*(i) 
      
      if ncol%2 == 0:
        tht = pi/2. - 2*alpha*(i-0.5)
      
      #Coordinates of the the center of the panel in XY projection
      x   = 0.5*(rout+rin) * cos(tht)
      y   = 0.5*(rout+rin) * sin(tht)
      
      #Same as before, inside a 1x2 numpy array
      XC  = np.array( [x,y] )
      
      #Panel coordinate system rotation matrix
      R = np.array([ [cos(-tht),sin(-tht)], [-sin(-tht),cos(-tht)] ])  
      #Corner XY coordinates in the panel coordinate system
      #Height
      h   = rout - rin
      #Corrected width, Tales theorem ;)
      wpr = w*rin/rout
      #4x2 numpy array containing the corners
      C   = np.array( [ [h/2,w/2],[h/2,-w/2],[-h/2,-wpr/2],[-h/2,wpr/2] ] )
      
      #Compute the corner positions in the global coordinate system
      C1,C2,C3,C4 = XC + dot(R,C[0]), XC + dot(R,C[1]), XC + dot(R,C[2]), XC + dot(R,C[3])

      #Build the 4x3 matrix containing the corners of each panel
      panel_corners = []
      for corner in [C1,C2,C3,C4]:
        
        x,y = corner[0],corner[1]
        z   = -Zr_primary(x,y)

        panel_corners.append( [x,y,z] )
      
      #4x3 matrix
      panel_corners = np.array( panel_corners )

      all_corners.append( panel_corners )
      
  return np.array( all_corners )


import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

all_corners = get_XYZ_corners()

for panel in all_corners:
  c = panel
  ax.plot( [c[0][0],c[1][0]], [c[0][1],c[1][1]], [c[0][2],c[1][2]], color='black' )
  ax.plot( [c[1][0],c[2][0]], [c[1][1],c[2][1]], [c[1][2],c[2][2]], color='black' )
  ax.plot( [c[2][0],c[3][0]], [c[2][1],c[3][1]], [c[2][2],c[3][2]], color='black' )
  ax.plot( [c[3][0],c[0][0]], [c[3][1],c[0][1]], [c[3][2],c[0][2]], color='black' )
 
pl.axis('equal')    
pl.show()




